#include "Basic.hpp"
#include <stdio.h>

#include "FreeRTOS.h"
#include "task.h"

#include "drv_Main.hpp"
#include "MS_Main.hpp"
#include "Parameters.hpp"
#include "FlashIO.h"
#include "Modes.hpp"
#include "MSafe.hpp"
#include "ctrl_Main.hpp"
#include "drv_PWMOut.hpp"
#include "AC_Math.hpp"
#include "Missions.hpp"
#include "ControlSystem.hpp"
#include "AuxFuncs.hpp"

#include "debug.hpp"

#if 1 

	__asm(".global __use_no_semihosting\n\t") ;
	extern "C"
	{


		void _sys_exit(int x)
		{
			x = x;
		}


		void _ttywrch(int ch)
		{
			ch = ch;
		}
		
		char *_sys_command_string(char *cmd, int len)
		{
				return 0;
		}
 
	}
#endif
	

#define Firmware_Version 116.9
	
volatile bool ESC_Calib_Completed = false;
void DriverInit_task(void* pvParameters)
{

	init_drv_Main();

	init_MS_Main();
	init_Debug();
	init_Modes();
	init_MSafe();
	init_ControlSystem();	
	

		struct
		{
			uint32_t calib_ESC[2];
			float calib_ESC_T[2];	
			uint32_t boot_count[2];
			float Firmvare_Version[2];	
		}init_cfg;
		init_cfg.calib_ESC[0] = 0;
		init_cfg.calib_ESC_T[0] = 3.5;
		init_cfg.boot_count[0] = 0;
		init_cfg.Firmvare_Version[0] = Firmware_Version;
		MAV_PARAM_TYPE param_types[] = {
			MAV_PARAM_TYPE_UINT32 ,	
			MAV_PARAM_TYPE_REAL32 ,	
			MAV_PARAM_TYPE_UINT32 , 
			MAV_PARAM_TYPE_REAL32   
			
		};
		SName param_names[] = {
			"Init_CalibESC",	 
			"Init_CalibESC_T", 
			"Init_Boot_Count", 
			"Init_Firmware_V"  
		};
		ParamGroupRegister( "Init", 1, sizeof(init_cfg)/8, param_types, param_names, (uint64_t*)&init_cfg );	

	

	while( getInitializationCompleted() == false )
	{
		setInitializationCompleted();
		os_delay(0.1);
	}
	
	uint8_t uav_type[8];
	if( ReadParam( "AC_UAVType", 0, 0, (uint64_t*)uav_type, 0 ) == PR_OK )
	{	
		UAV_MTCount mt_count = UAV_MainMotorCount(uav_type[0]);
		set_MainMotorCount( mt_count.MTCount, mt_count.STCount );
	}
	


		ReadParamGroup( "Init", (uint64_t*)&init_cfg, 0 );
	

		if( init_cfg.calib_ESC[0] == 21586 )
		{
			MainMotor_PullUpAll();
			os_delay(init_cfg.calib_ESC_T[0]);
			MainMotor_PullDownAll();
			
			init_cfg.calib_ESC[0] = 0;
		}
		

		init_cfg.boot_count[0]++;
		init_cfg.Firmvare_Version[0] = Firmware_Version;
		UpdateParamGroup( "Init", (uint64_t*)&init_cfg, 0, sizeof(init_cfg)/8 );
		
		MainMotor_PullDownAll();
		ESC_Calib_Completed = true;
	
	init_Missions();
		
	vTaskDelete(0);
}
	
int main(void)
{	
  init_Basic();
			
	xTaskCreate( DriverInit_task , "Init" ,8192,NULL,3,NULL);
	vTaskStartScheduler();
	while(1);
}

extern "C" void HardFault_Handler()
{

	PWM_PullDownAll();
}

extern "C" void vApplicationStackOverflowHook( TaskHandle_t xTask, signed char *pcTaskName )
{
	static char StackOvTaskName[20];
	strcpy( StackOvTaskName, (char*)pcTaskName );
}



